#include "ros/ros.h"
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include "sensor_msgs/JointState.h"
#include <string>
#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>
#include <iostream>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_listener.h>
#include <tf/tf.h>
#include <cmath>
//#include "msg_swap.h"
// #include "zhuifeng/QR.h"
#include <queue> 

using namespace std;
serial::Serial ros_ser;       //声明串口对象
constexpr double per_speed=0.024;    // 单位代码速度的真实速度0.024m/s
constexpr double speed_lost=25;
constexpr double time_lost_length = 3; //允许小车丢失时间
// 转向角速度是 30代码/秒速度 5秒实际转向180°
// 1代码角速度/s 等于1.2°/s 实际速度
constexpr double per_conP = 1.2;

constexpr double e=2.71;
class Flag
{
public:
    int count;
};



int main (int argc, char** argv)
{
    ros::init(argc, argv, "my_con");
    ros::NodeHandle n;          
    
    tf::TransformListener listenser;
    tf::StampedTransform transform;
    tf::StampedTransform transform_temp;
    tf::StampedTransform transform_fromer;
    queue<tf::StampedTransform> queue_tran;
    int count=0;
    double time_lost= 0;
    double time_length= 0;
    signed int con_P = 0, Speed= 0; // con_p用于左转右转， speed用于速度
    signed int move_to_left_or_right = 0;   // 用于平移标示
    cout<<"lintenser is here "<<endl;
    
    // try 
    // {
    //      //设置串口属性，并打开串口/dev/ttyUSB0
    //      ros_ser.setPort("/dev/ttyUSB0");//
    //      ros_ser.setBaudrate(115200);
    //      serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    //      ros_ser.setTimeout(to);
    //      ros_ser.open();
    // }
    //  catch (serial::IOException& e)
    //  {
    //      ROS_ERROR_STREAM("Unable to open port ");
    //      return -1;
    //  }
    //  //检测串口是否已经打开，并给出提示`信息 
    //  if(ros_ser.isOpen()){
    //      ROS_INFO_STREAM("Serial Port opened");
    //  }else{
    //      return -1;
    //  }
    ros::Rate loop_rate(10);
    
    

/**************************************************************/
//初始化
    //第一位平移 正数右平移
    //第二位前进后退，正数向前
    //第三位转向，正数右转，
    std::stringstream data;
    char Data[10]={0,0,0,0,0,0,0,0,(char)0xff,(char)0xfe};
    data  <<Data[8] <<Data[0] << Data[1] <<Data[2]<< Data[3] << Data[4] << Data[5] << Data[6] << Data[7] <<Data[9];
    // ros_ser.write(data.str()); 
/**************************************************************/
    
    double speed_add = 0;
    double speed_Z = 0;
    double speed_current = 0; 
    double z_add = 0;
    while(ros::ok())
    {
        //仿真时候用/cam话题 真跑时候用/usb_cam 话题
        listenser.waitForTransform("/cam", "my_tag", ros::Time(0), ros::Duration(4.0));
        try
        {
            cout<<"listen is run "<<endl;
            transform_temp = transform; //上一个tf
            listenser.lookupTransform("/cam", "my_tag", ros::Time(0), transform);


            queue_tran.push(transform);
            if(queue_tran.size()>10)
            {
                transform_fromer=queue_tran.front();
                queue_tran.pop();
                
            }
            
            //开始跟踪

            double Q_w=transform.getRotation().getW();
            double Q_x=transform.getRotation().getX();
            double Q_y=transform.getRotation().getY();
            double Q_z=transform.getRotation().getZ();

                // double flag_con_P = 0;
                // flag_con_P = (asin(2*(Q_w*Q_y+Q_x*Q_z)))*180/M_PI;
                // cout<<" flag_conP = "<<flag_con_P<<endl;

            if( !(transform_temp==transform)) //没有丢失目标 
            {
                
                // cout<<" new conP "<<con_P<<endl;
                double z = transform.getOrigin().z(); //记录当前距离
                double z_former = transform_temp.getOrigin().z();
                double t = transform.stamp_.sec;
                double t_former = transform_temp.stamp_.sec;
                //根据相对距离计算递增速度
                z_add = z-z_former;
                speed_add = (z-z_former)*10;
                //根据绝对距离计算速度增量
                speed_Z = (z-1.0)*10;
                
                //叠加递增速度
                speed_current = speed_Z + speed_add;
                


                cout<<" speed_Current is "<<speed_current<<endl;
                cout<<" speed_add is " << speed_add <<endl;

                //一次性计算角速度
                con_P = (atan2(transform.getOrigin().x(), transform.getOrigin().z())*180/M_PI)/per_conP;
                //赋值线速度
                Speed = speed_current;
                //test git 
                count = 0;
            }

            else if(transform_temp==transform) //丢失目标了
            {
                ++count;
                cout<< "count "<< count <<endl;
                if(count==1)
                {
                    
                    time_lost=ros::Time::now().toSec();
                    double z=transform.getOrigin().z();
                    time_length = z/(per_speed*speed_current);
                    
                    con_P = (atan2(transform.getOrigin().x(), transform.getOrigin().z())*180/M_PI)/per_conP;//这个角度转换太慢了 
                }
                else
                {
                    con_P = 0;
                    cout<<"test"<<endl;
                }
                double time_now=ros::Time::now().toSec();
                Speed=speed_current;

                if(time_now-time_lost>time_length)
                {
                    Speed=0;
                    
                    speed_current = 0; //当前速度重置

                    double flag_con_P = 0;
                    flag_con_P = (asin(2*(Q_w*Q_y+Q_x*Q_z)))*180/M_PI;
                    cout<<" flag_conP = "<<flag_con_P<<endl;
                    if(flag_con_P<-5)
                    {
                        con_P = 40/per_conP;
                    }
                    if(flag_con_P>5)
                    {
                        con_P = -(40/per_conP);
                    }
                }
                if(time_now - time_lost > time_length+2)
                {
                    cout<<"丢失目标"<<endl;
                    // break;
                    continue;
                }
            }
            
            
            

        }
        catch(const tf::TransformException &e)
        {
            std::cerr << e.what() << '\n';
            ros::Duration(1.0).sleep();
            continue;
        }
        
        
        if(con_P > 60) con_P = 60;
        if(con_P < -60) con_P = -60;
        if(Speed > 127) Speed = 127;
        if(Speed <0) Speed = 0;

        std::cout<<"输出的位置坐标：x="<<transform.getOrigin().x()<<",y="<<transform.getOrigin().y()<<",z="<<transform.getOrigin().z()<<std::endl;
        std::cout<<"输出的旋转四元数：w="<<transform.getRotation().getW()<<",x="<<transform.getRotation().getX()<<
            ",y="<<transform.getRotation().getY()<<",z="<<transform.getRotation().getZ()<<std::endl;
        cout<<"speed "<< Speed << "con_P " << con_P<<endl;
        std::stringstream data;
        char Data[10]={(char)move_to_left_or_right,(char)Speed,(char)con_P,0,0,0,0,0,(char)0xff,(char)0xfe};//data  
        data  <<Data[8] <<Data[0] << Data[1] <<Data[2]<< Data[3] << Data[4] << Data[5] << Data[6] << Data[7] <<Data[9];
        // ros_ser.write(data.str());         //发送串口数据
        std::cout<<" 123 "<<std::endl;
        con_P = 0;
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
